// ======================================================================
// Copyright (C) 2013 Hell-Prototypes. / www.hellprototypes.com
//
// This program is free software; you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation; either version 2 of the License, or (at
// your option) any later version.
//
// This program is distributed in the hope that it will be useful, but
// WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
// General Public License for more details.
// ======================================================================
#include "vision_thread.h"

#define AREA_LIMIT_FACTOR    5
#define ALF_MIN (((double)AREA_LIMIT_FACTOR-1)/(double)AREA_LIMIT_FACTOR)
#define ALF_MAX (((double)AREA_LIMIT_FACTOR+1)/(double)AREA_LIMIT_FACTOR)

static int c_default_capture_property[8][2] = {
    {10, 0}, {11, 32},   {12, 50},  {13, 0}, {14, 0}, {20, 4},    {22, 220}, {32, 0},
};

vision_thread::vision_thread(QObject *parent) : QThread(parent)
{
    m_loop_run = true;
    m_debug_image_en = false;

    m_pCapture = NULL;
    m_pFrame = NULL;

    m_threshold = 220;
    m_report_parts_position = false;

    m_image_w = -1;
    m_image_h = -1;

    m_target_part_area = 0;

    cvInitFont(&m_cvfont, CV_FONT_HERSHEY_SIMPLEX, 0.8, 0.8, 0, 2, 8);
}

vision_thread::~vision_thread()
{
    if(m_pCapture != NULL) {
        cvReleaseCapture( &m_pCapture );
    }
}

bool vision_thread::set_capture_index(int index)
{
    if(m_pCapture != NULL) {
        cvReleaseCapture( &m_pCapture );
        m_pCapture = NULL;
    }
    m_pCapture = cvCreateCameraCapture(index);
    if(m_pCapture) {
        m_pFrame = cvQueryFrame( m_pCapture );
        if( m_pFrame ) {
            m_image_w = m_pFrame->width;
            m_image_h = m_pFrame->height;
            set_feeder_zone_size(m_feeder_zone_size);
            emit camera_image_size(m_image_w, m_image_h);

            return true;
        }
    }

    return false;
}

double vision_thread::get_capture_property( int property_id )
{
    double ret = -1;

    if(m_pCapture) {
        ret = cvGetCaptureProperty(m_pCapture, property_id);
    }

    return ret;
}

int vision_thread::set_capture_property( int property_id, double value )
{
    int ret = -1;

    if(m_pCapture) {
        ret = cvSetCaptureProperty( m_pCapture, property_id, value );
    }

    return ret;
}
//Default property value:
//  id = 3, property = 640      WIDTH
//  id = 4, property = 480      HEIGHT
//  id = 5, property = 0        FPS
//  id = 10, property = 0       BRIGHTNESS  //亮度    - 0~127
//  id = 11, property = 32      CONTRAST    //对比度  - 0~64
//  id = 12, property = 50      SATURATION  //饱和度  - 0~100
//  id = 13, property = 0       HUE         //色度    - 0~180
//  id = 14, property = 0       GAIN        //增益    - 0~100
//  id = 17, property = 4650    WHITE_BALANCE_BLUE_U
//  id = 20, property = 4       SHARPNESS   //清晰度   - 0~8
//  id = 22, property = 220     GAMMA       //灰度系数 - 100~500
//  id = 32, property = 0       BACKLIGHT   //背光    - 0~1
bool vision_thread::reset_capture_property()
{
    if(m_pCapture) {
        //Set default capture property
        for(int i=0; i<8; i++) {
            if(cvGetCaptureProperty(m_pCapture, c_default_capture_property[i][0]) != c_default_capture_property[i][1]) {
                if(cvSetCaptureProperty( m_pCapture, c_default_capture_property[i][0],
                                                     c_default_capture_property[i][1] ) < 0) {
                    return false;
                }
            }
        }
        return true;
    }
    return false;
}

void vision_thread::set_feeder_zone_size(QSize size)
{
    m_roi_mutex.lock();
    m_feeder_zone_size = size;

    m_roi_gray.x = (m_image_w - m_feeder_zone_size.width())/2;
    m_roi_gray.y = (m_image_h - m_feeder_zone_size.height())/2;
    m_roi_gray.width = m_feeder_zone_size.width();
    m_roi_gray.height = m_feeder_zone_size.height();

    double k1 = (double)m_image_w/(double)m_image_h;
    double k2 = (double)m_feeder_zone_size.width()/(double)m_feeder_zone_size.height();
    if(k1 > k2) {
        m_roi_bin_zoom_factor = (double)m_image_h/(double)m_feeder_zone_size.height();
        m_roi_bin.width  = m_roi_bin_zoom_factor * (double)m_feeder_zone_size.width();
        m_roi_bin.height = m_image_h;
        m_roi_bin.x = (m_image_w - m_roi_bin.width)/2;
        m_roi_bin.y = 0;
    } else {
        m_roi_bin_zoom_factor = (double)m_image_w/(double)m_feeder_zone_size.width();
        m_roi_bin.width  = m_image_w;
        m_roi_bin.height = m_roi_bin_zoom_factor * (double)m_feeder_zone_size.height();
        m_roi_bin.x = 0;
        m_roi_bin.y = (m_image_h - m_roi_bin.height)/2;
    }
    m_roi_mutex.unlock();
}

void vision_thread::run()
{
    if(m_pCapture == NULL) {
        return;
    }

    vector<part_t> parts_pos;
    m_pFrame = cvQueryFrame( m_pCapture );

    IplImage* bin_image = cvCreateImage(cvSize(m_image_w,m_image_h),IPL_DEPTH_8U,1);
    IplImage* gray_image = cvCreateImage(cvSize(m_image_w,m_image_h),IPL_DEPTH_8U,1);

    do {
        m_pFrame = cvQueryFrame( m_pCapture );
        if( !m_pFrame ) break;
        parts_pos.clear();

        m_roi_mutex.lock();
        cvResetImageROI(gray_image);
        cvCvtColor(m_pFrame ,gray_image, CV_BGR2GRAY);

        cvSetImageROI(bin_image, m_roi_bin);

        cvSetImageROI(gray_image, m_roi_gray);
        cvResize(gray_image, bin_image, CV_INTER_CUBIC);

        cvThreshold(bin_image, bin_image, m_threshold, 255, CV_THRESH_BINARY); //CV_THRESH_OTSU
        cvErode( bin_image, bin_image, NULL, 1 );
        hole_fill(bin_image);
        cvDilate( bin_image, bin_image, NULL, 1 );
        cvSmooth( bin_image, bin_image, CV_MEDIAN, 3, 0, 0, 0);

        find_box(parts_pos, bin_image);
        draw_part_flag(m_pFrame, parts_pos);

        draw_feeder_zone(m_pFrame);

        emit frame_update(m_pFrame);
        if(m_report_parts_position) {
            if(parts_pos.size() > 0) {
                m_report_parts_position = false;
                m_parts_position_cache = parts_pos;
                emit parts_position_info(&m_parts_position_cache);
            }
        }

        if(m_debug_image_en) {
            cvSetImageROI(gray_image, m_roi_bin);
            cvCopy(bin_image, gray_image);
            emit debug_image_update("Parts Binary Image" , gray_image);
        }
        m_roi_mutex.unlock();

        if(m_loop_run) {
            msleep(200);
        }
    }while(m_loop_run);

    cvReleaseImage( &bin_image );
    cvReleaseImage( &gray_image );
}

double vision_thread::angle( Point pt1, Point pt2, Point pt0 )
{
    double dx1 = pt1.x - pt0.x;
    double dy1 = pt1.y - pt0.y;
    double dx2 = pt2.x - pt0.x;
    double dy2 = pt2.y - pt0.y;
    return (dx1*dx2 + dy1*dy2)/sqrt((dx1*dx1 + dy1*dy1)*(dx2*dx2 + dy2*dy2) + 1e-10);
}

double vision_thread::angle( Point pt0, Point pt1)
{
    return atan2(pt0.y - pt1.y, pt0.x - pt1.x);
}

void vision_thread::draw_box(CvBox2D &box,IplImage* img)
{
    CvPoint2D32f point[4];

    for (int i=0; i<4; i++) {
        point[i].x = 0;
        point[i].y = 0;
    }
    cvBoxPoints(box, point);
    CvPoint pt[4];
    for (int i=0; i<4; i++)
    {
        pt[i].x = (int)point[i].x;
        pt[i].y = (int)point[i].y;
    }

    cvLine( img, pt[0], pt[1],CV_RGB(128,128,128), 2, 8, 0 );
    cvLine( img, pt[1], pt[2],CV_RGB(128,128,128), 2, 8, 0 );
    cvLine( img, pt[2], pt[3],CV_RGB(128,128,128), 2, 8, 0 );
    cvLine( img, pt[3], pt[0],CV_RGB(128,128,128), 2, 8, 0 );
}

void vision_thread::find_box(vector<part_t> & parts_pos, IplImage * image)
{
    CvMemStorage *storage;
    CvSeq * pcontour = 0;
    IplImage *ipl_image;
    CvBox2D box_2D;
    part_t part_pos;
    int mode = CV_RETR_LIST; //CV_RETR_LIST, CV_RETR_EXTERNAL
    double contour_area = 0.0, box_area = 0.0;
    double min_area = m_image_w*m_image_h, max_area = 0.0, total_area = 0.0;
    int    box_count = 0;

    storage = cvCreateMemStorage(0);
    ipl_image = cvCloneImage(image);
    cvFindContours(ipl_image, storage, &pcontour, sizeof(CvContour), mode, CV_CHAIN_APPROX_NONE);

    for (; pcontour != 0; pcontour=pcontour->h_next) {
        box_2D = cvMinAreaRect2(pcontour);
        contour_area = cvContourArea(pcontour, CV_WHOLE_SEQ);
        box_area = box_2D.size.width * box_2D.size.height;
        if((contour_area > 500) && (contour_area < (image->width * image->height)/2) && (contour_area > box_area/2)) {
            if(box_area < min_area) { min_area = box_area; }
            if(box_area > max_area) { max_area = box_area; }
            total_area += box_area;
            box_count++;
            if((m_target_part_area == 0)
               || ((box_area > m_target_part_area * ALF_MIN) && (box_area < m_target_part_area * ALF_MAX))) {//area limit
                draw_box(box_2D, image);
                part_pos.center.x = box_2D.center.x/m_roi_bin_zoom_factor  + (m_image_w - m_feeder_zone_size.width())/2;
                part_pos.center.y = box_2D.center.y/m_roi_bin_zoom_factor  + (m_image_h - m_feeder_zone_size.height())/2;
                /*correction box angle for Hell PAP*/
                if(box_2D.size.height > box_2D.size.width) {
                    box_2D.angle += 90; //add 90 degree if height greater than width
                }
                part_pos.angle = box_2D.angle * CV_PI/180;
                parts_pos.push_back(part_pos);
            }
        }
    }

    cvReleaseMemStorage(&storage);
    cvReleaseImage(&ipl_image);

    m_detected_parts_info.parts_num = parts_pos.size();
    m_detected_parts_info.parts_min_area = (box_count > 0) ? min_area : 0;
    m_detected_parts_info.parts_max_area = max_area;
    m_detected_parts_info.parts_avr_area = total_area/(box_count + 1e-10);
}
void vision_thread::hole_fill( IplImage * image)
{
    CvMemStorage *storage;
    CvSeq * pcontour = 0;
    CvScalar external_color=cvScalarAll(0);
    CvScalar hole_color=cvScalarAll(128);
    int mode = CV_RETR_LIST;//CV_RETR_EXTERNAL;
    double area = 0.0;
    IplImage *ipl_image;

    storage = cvCreateMemStorage(0);
    ipl_image = cvCloneImage(image);
    cvFindContours(ipl_image, storage, &pcontour,sizeof(CvContour), mode, CV_CHAIN_APPROX_NONE);

    for (; pcontour != 0; pcontour=pcontour->h_next) {
        area = cvContourArea(pcontour, CV_WHOLE_SEQ);
        if(area < 500) {
            cvDrawContours(image, pcontour, external_color, hole_color,0,CV_FILLED);//CV_FILLED
        }
    }

    cvReleaseMemStorage(&storage);
    cvReleaseImage(&ipl_image);
}

#define CROSS_SHAP_SIZE     20
void vision_thread::draw_feeder_zone( IplImage * image)
{
    CvPoint pl, pr;

    int w = m_feeder_zone_size.width();
    int h = m_feeder_zone_size.height();
    pl.x = image->width/2 - w/2;
    pl.y = image->height/2 - h/2;
    pr.x = pl.x + w;
    pr.y = pl.y + h;
    cvRectangle( image, pl, pr, Scalar(0,0,255), 1);

    pl.x = image->width/2;
    pl.y = image->height/2 - CROSS_SHAP_SIZE/2;
    pr.x = image->width/2;
    pr.y = image->height/2 + CROSS_SHAP_SIZE/2;
    cvLine( image, pl, pr, Scalar(0,255,255), 1);

    pl.x = image->width/2 - CROSS_SHAP_SIZE/2;
    pl.y = image->height/2;
    pr.x = image->width/2 + CROSS_SHAP_SIZE/2;
    pr.y = image->height/2;
    cvLine( image, pl, pr, Scalar(0,255,255), 1);
}

void vision_thread::draw_part_flag( IplImage * image, const vector<part_t>& parts_pos )
{
    char text[128];

    CvFont font = cvFont(0.9);
    sprintf(text, "%d PCS, Min: %d, Max: %d, Av: %d",
                  m_detected_parts_info.parts_num,      m_detected_parts_info.parts_min_area,
                  m_detected_parts_info.parts_max_area, m_detected_parts_info.parts_avr_area );
    cvPutText(image, text, cvPoint(1, m_image_h-8), &font, CV_RGB(255,255,0));

    for(size_t i=0; i<parts_pos.size(); i++) {
        CvPoint tip;
        double angle = parts_pos[i].angle;

        tip.x = parts_pos[i].center.x + cos(angle)*24;
        tip.y = parts_pos[i].center.y + sin(angle)*24;
        cvLine( image, parts_pos[i].center, tip, Scalar(0,255,0), 1);

#if 1
        cvCircle(image, parts_pos[i].center, 3, CV_RGB( 255, 0, 0 ) );

#else
        CvPoint pt;
        pt.x = parts_pos[i].center.x + cos(angle+3.14/9)*20;
        pt.y = parts_pos[i].center.y + sin(angle+3.14/9)*20;
        cvLine( image, tip, pt, Scalar(0,255,0), 1);

        pt.x = parts_pos[i].center.x + cos(angle-3.14/9)*20;
        pt.y = parts_pos[i].center.y + sin(angle-3.14/9)*20;
        cvLine( image, tip, pt, Scalar(0,255,0), 1);
#endif
    }
}

